#include "coreController.h"
#include "scene.h"
#include "Instance.h"

CoreController::CoreController()
{
	targeId -1;
}

controllerPerseguirEquipo::controllerPerseguirEquipo()
{
	targeId = -1;
	distAnt = 0;
	this->targetPos = Vector3(0,0,0);
}

void controllerPerseguirEquipo::recalc(Ship * _ship, double _seconds_elapsed)
{
	if(!ENABLE_CONTROLLERS) return;
	ObtenerObjetivo(_ship);
	if (_ship->isPlayer) return;
	if (targetId == -1) return;
	int _object = owner;
	int _layer = 1;
	int targetLayer = 1;

	Scene * scene = Scene::getScene();
	Ship * ship2 = (Ship*) scene->getShipById(targetId);
	Vector3 myPos = _ship->model->getPosition();
	targetOldPos = targetPos;
	Vector3 targetPos = ship2->model->getPosition();
	targetSpeed = targetPos - targetOldPos;

	float distance = abs(getDistance(myPos,targetPos));
	if (distance < _ship->vision/2) _ship->Move("accelerate",100,0);
	else if (distance > _ship->vision/2) _ship->Move("break",100,0);
	//objecto a mover 

	if(_ship->alive && abs(distance) < _ship->vision )
	{
		Matrix44 * m = _ship->model;
		Vector3 f = m->frontVector();
		Vector3 r = m->rightVector();
		Vector3 u = m->topVector();
		Vector3 p = m->getPosition();

		float vel_ang = ((float)_seconds_elapsed * 1.0f);
		
		//PLANO IZQUIERDA/DERECHA
		float A = r.x;
		float B = r.y;
		float C = r.z;
		float D = -(r.x*p.x + r.y*p.y + r.z*p.z) ;
		float resLR, resUD;
		resLR = A*targetPos.x +B*targetPos.y +C*targetPos.z + D;

		int mov = 1;
		if (getDistance(myPos,targetPos) < 50) 
			mov = -1;

		//PLANO ARRIBA/ABAJO
		A = u.x;
		B = u.y;
		C = u.z;
		D =-( u.x*p.x + u.y*p.y + u.z*p.z) ;
		resUD = A*targetPos.x + B*targetPos.y + C*targetPos.z + D;
		
		if (resLR > 0){
			//doblar derecha
				_ship->Move("right",80*mov, _seconds_elapsed);
		}
		else if (resLR < 0) {
			//doblar izquierda
				_ship->Move("left",80*mov, _seconds_elapsed);

		}
		if (resUD > 0){
			//SUBIR
			_ship->Move("up",80*mov, _seconds_elapsed);

		}
		else if (resUD < 0) {
			//bajar
			_ship->Move("down",80*mov, _seconds_elapsed);

		}
		//derecha arriba/abajo
		if(resLR > 0 && resUD != 0) _ship->Move("roll_right",50*mov, _seconds_elapsed);
		//izquierda arriba/abajo
		if(resLR < 0 && resUD != 0) _ship->Move("roll_left",50*mov, _seconds_elapsed);

			_ship->Move("forward",80, _seconds_elapsed);

		//Comprobamos si tiene que disparar
	//		Vector3 FireLine = getFireLine(_object, targetPos, targetSpeed);
		Vector3 FireLine = getFireLine(_object, targetPos);
		if( isOnFireLine(_object, FireLine, 0.9999) ) {
			fireTarget(_object, FireLine, 3);
		}
		else if( isOnFireLine(_object, FireLine, 0.95) ) {
			fireTarget(_object, FireLine, 2);
		}
		else if( isOnFireLine(_object, FireLine, 0.90) ) {
			fireTarget(_object, FireLine, 1);
		}
	}
}
void controllerPerseguirEquipo::ObtenerObjetivo(Ship * _ship)
{
	std::string team = _ship->getTag("team");
	Ship * s2;
	bool buscar = false;
	Scene * s = Scene::getScene();
	s2 = s->getShipById(targetId);
	if(s2 != NULL){
		if((s2 == NULL) || (!s2->alive) || abs(getDistance(_ship->model->getPosition(),s2->model->getPosition())) > _ship->vision){
			//buscar, objetivo muerto
			buscar = true;
		}
	} else {
		//buscar, objetivo inexistente
		buscar = true;
	}
	if (buscar){
		targetId = -1;
		double dist,dist_min;
		Vector3 p1,p2;
		p1 =  *_ship->model * Vector3(0,0,0);
		std::string t1, t2;
		t1 = _ship->getTag("team");
		dist_min = _ship->vision;
		for (int j = 0; j < s->instances_layer1.size(); j++){
			s2 = (Ship*)s->instances_layer1[j];
			p2 = s2->model->getPosition();
			dist = getDistance(p1,p2);
			t2 =  s2->getTag("team");
			if((abs(dist) < abs(dist_min)) &&(abs(dist) < _ship->vision) && (t1 != "") && (t1 != t2 )){
				dist_min = dist;
				targetId = s2->id_instance;
				return;
			}
		}
	}
}




//===========================================================================================
//=================     PREDICTIVO       ====================================================
//===========================================================================================

controllerPerseguirEquipoPred::controllerPerseguirEquipoPred()
{
	targeId = -1;
	distAnt = 0;
	this->targetPos = Vector3(0,0,0);
}

void controllerPerseguirEquipoPred::recalc(Ship * _ship, double _seconds_elapsed)
{
	if (!ENABLE_CONTROLLERS) return;
	ObtenerObjetivo(_ship);
	if (_ship->isPlayer) return;
	if (targetId == -1) return;
	int _object = owner;
	int _layer = 1;
	int targetLayer = 1;

	Scene * scene = Scene::getScene();
	Ship * ship2 = (Ship*) scene->getShipById(targetId);
	Vector3 myPos = _ship->model->getPosition();
	targetOldPos = targetPos;
	Vector3 targetPos = ship2->model->getPosition();
	targetSpeed = targetPos - targetOldPos;

	float distance = abs(getDistance(myPos,targetPos));
	if (distance < _ship->vision/2) _ship->Move("accelerate",100,0);
	else if (distance > _ship->vision/2) _ship->Move("break",100,0);
	//objecto a mover 

	if(_ship->alive && abs(distance) < _ship->vision )
	{
		Matrix44 * m = _ship->model;
		Vector3 f = m->frontVector();
		Vector3 r = m->rightVector();
		Vector3 u = m->topVector();
		Vector3 p = m->getPosition();

		float vel_ang = ((float)_seconds_elapsed * 1.0f);
		
		//PLANO IZQUIERDA/DERECHA
		float A = r.x;
		float B = r.y;
		float C = r.z;
		float D = -(r.x*p.x + r.y*p.y + r.z*p.z) ;
		float resLR, resUD;
		resLR = A*targetPos.x +B*targetPos.y +C*targetPos.z + D;

		int mov = 1;
		if (getDistance(myPos,targetPos) < 50) 
			mov = -1;

		//PLANO ARRIBA/ABAJO
		A = u.x;
		B = u.y;
		C = u.z;
		D =-( u.x*p.x + u.y*p.y + u.z*p.z) ;
		resUD = A*targetPos.x + B*targetPos.y + C*targetPos.z + D;
		
		if (resLR > 0){
			//doblar derecha
				_ship->Move("right",80*mov, _seconds_elapsed);
		}
		else if (resLR < 0) {
			//doblar izquierda
				_ship->Move("left",80*mov, _seconds_elapsed);

		}
		if (resUD > 0){
			//SUBIR
			_ship->Move("up",80*mov, _seconds_elapsed);

		}
		else if (resUD < 0) {
			//bajar
			_ship->Move("down",80*mov, _seconds_elapsed);

		}
		//derecha arriba/abajo
		if(resLR > 0 && resUD != 0) _ship->Move("roll_right",50*mov, _seconds_elapsed);
		//izquierda arriba/abajo
		if(resLR < 0 && resUD != 0) _ship->Move("roll_left",50*mov, _seconds_elapsed);

			_ship->Move("forward",80, _seconds_elapsed);

		//Comprobamos si tiene que disparar
		Vector3 FireLine = getFireLinePredictive(_object, targetPos, targetSpeed);
		if( isOnFireLine(_object, FireLine, 0.98) ) {
			fireTarget(_object, FireLine, 3);
		}
		else if( isOnFireLine(_object, FireLine, 0.95) ) {
			fireTarget(_object, FireLine, 2);
		}
		else if( isOnFireLine(_object, FireLine, 0.92) ) {
			fireTarget(_object, FireLine, 1);
		}
	}
}

void controllerPerseguirEquipoPred::ObtenerObjetivo(Ship * _ship)
{
	std::string team = _ship->getTag("team");
	Ship * s2;
	bool buscar = false;
	Scene * s = Scene::getScene();
	s2 = s->getShipById(targetId);
	if(s2 != NULL){
		if((s2 == NULL) || (!s2->alive) || abs(getDistance(_ship->model->getPosition(),s2->model->getPosition())) > _ship->vision){
			//buscar, objetivo muerto
			buscar = true;
		}
	} else {
		//buscar, objetivo inexistente
		buscar = true;
	}
	if (buscar){
		targetId = -1;
		double dist;
		double dist_min;
		Vector3 p1,p2;
		p1 =  *_ship->model * Vector3(0,0,0);
		std::string t1, t2;
		t1 = _ship->getTag("team");
		dist_min = _ship->vision;
		for (int j = 0; j < s->instances_layer1.size(); j++){
			s2 = (Ship*)s->instances_layer1[j];
			p2 = s2->model->getPosition();
			dist = getDistance(p1,p2);
			t2 =  s2->getTag("team");
			if((abs(dist) < abs(dist_min)) &&(abs(dist) < _ship->vision) && (t1 != "") && (t1 != t2 )){
				dist_min = dist;
				targetId = s2->id_instance;

			}
		}
	}
}